﻿using System;
using System.Collections.Generic;
using Petra.Gis.Geometry.Support;

namespace testSRST
{
  class testEqHelmert3d
  {

    public void Tran3D(Coordinate P, Vector rot, Vector pos, double m,
    double a, double e2, double A, double E2, double E22)
    {

      Coordinate outP = new Coordinate();

      //rot se vkladaji ve vterinach
      double N, W, h;

      //const double DEG2RAD = 57.2957795130823;
      const double DEG2RAD = 180 / Math.PI;
      
      //const double SEC_TO_RAD = 4.8481368110953599358991410235795e-6;
      const double SEC_TO_RAD = 1 / 3600 / DEG2RAD;

      //point in,out,
      Coordinate in3D = new Coordinate(), out3D = new Coordinate();

      //rot.I = rot.I / 3600 / DEG2RAD;
      //rot.J = rot.J / 3600 / DEG2RAD;
      //rot.K = rot.K / 3600 / DEG2RAD;
      rot.I = rot.I * SEC_TO_RAD;
      rot.J = rot.J * SEC_TO_RAD;
      rot.K = rot.K * SEC_TO_RAD;

      //in=P;
      P.X = (P.X) / DEG2RAD;
      P.Y = (P.Y) / DEG2RAD;
      
      W = Math.Sqrt(1 - e2 * Math.Pow(Math.Sin(P.X), 2));
      N = a / W;
      h = 230;

      in3D.X = (N + h) * Math.Cos(P.X) * Math.Cos(P.Y);
      in3D.Y = (N + h) * Math.Cos(P.X) * Math.Sin(P.Y);
      in3D.Z = (N * (1 - e2) + h) * Math.Sin(P.X);

      out3D.X = (1 + m) * (in3D.X + rot.K * in3D.Y - rot.J * in3D.Z) + pos.I;
      out3D.Y = (1 + m) * (-rot.K * in3D.X + in3D.Y + rot.I * in3D.Z) + pos.J;
      out3D.Z = (1 + m) * (rot.J * in3D.X - rot.I * in3D.Y + in3D.Z) + pos.K;

      outP.Y = Math.Atan(out3D.Y / out3D.X) * DEG2RAD;
      outP.X = Math.Atan(out3D.Z * (1 + E22) / Math.Sqrt((out3D.X * out3D.X) + (out3D.Y * out3D.Y)));

      W = Math.Sqrt(1 - E2 * Math.Pow(Math.Sin(outP.X), 2));
      N = A / W;

      // iterace
      for (int t = 0; t <= 5; t++)
      {
        outP.X = Math.Atan((out3D.Z + N * E2 * Math.Sin(outP.X)) / Math.Sqrt((out3D.X * out3D.X) + (out3D.Y * out3D.Y)));
        W = Math.Sqrt(1 - E2 * Math.Pow(Math.Sin(outP.X), 2));
        N = A / W;
        //i=i+1;
      }
      outP.X = outP.X * DEG2RAD;
      P = outP;
    }



  }
}
